
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mavproxy_common.c
  * @author     baiyang
  * @date       2021-8-25
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mavproxy_monitor.h"

#include "mavproxy.h"
#include "mavcmd.h"

#include <c_library_v2/ardupilotmega/mavlink.h>

#include <copter/fms.h>
#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static const float magic_force_arm_value = 2989.0f;
static const float magic_force_disarm_value = 21196.0f;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void mavproxy_send_command_ack(uint16_t command, uint8_t result, 
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};
    mavlink_system_t sysid_this_mav = mavproxy_get_system();

    mavlink_msg_command_ack_encode(sysid_this_mav.sysid, sysid_this_mav.compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

void mavproxy_send_command_ack_sysid(uint16_t command, uint8_t result,
    mavlink_message_t* msg)
{
    mavlink_command_ack_t command_ack = {command, result, 0, 0, 0, 0};

    mavlink_msg_command_ack_encode(msg->sysid, msg->compid,
        msg, &command_ack);

    mavproxy_send_immediate_msg(msg, 1);
}

#if 0
void mavproxy_proc_command(mavlink_command_long_t* command,
    mavlink_message_t* msg)
{
    MAV_RESULT result = MAV_RESULT_FAILED;
    bool ignore = true;

    switch (command->command) {
    case MAV_CMD_PREFLIGHT_CALIBRATION: {
        ignore = false;

        if (command->param1 == 1) { // calibration gyr
            mavcmd_set(MAVCMD_CALIBRATION_GYR, NULL);
        } else if (command->param2 == 1) { // calibration mag
            mavcmd_set(MAVCMD_CALIBRATION_MAG, NULL);
        } else if (command->param5 == 1) { // calibration acc
            mavcmd_set(MAVCMD_CALIBRATION_ACC, NULL);
        } else if (command->param5 == 2) { // calibration level
            // mavproxy_send_statustext_msg(CAL_START_LEVEL, msg);
        } else {
            /* all 0 command, cancel current process */
        }

        result = MAV_CMD_ACK_OK | MAV_CMD_ACK_ENUM_END;
        break;
    }

    default:
        break;
    }

    if (!ignore) {
        mavproxy_send_command_ack(command->command, result, msg);
    }
}
#endif

// return MAV_TYPE corresponding to frame class
MAV_TYPE mavproxy_get_frame_mav_type()
{
    switch ((motor_frame_class)PARAM_GET_INT8(VEHICLE,FRAME_CLASS)) {
        case MOTOR_FRAME_QUAD:
        case MOTOR_FRAME_UNDEFINED:
            return MAV_TYPE_QUADROTOR;
        case MOTOR_FRAME_HEXA:
        case MOTOR_FRAME_Y6:
            return MAV_TYPE_HEXAROTOR;
        case MOTOR_FRAME_OCTA:
        case MOTOR_FRAME_OCTAQUAD:
            return MAV_TYPE_OCTOROTOR;
        case MOTOR_FRAME_HELI:
        case MOTOR_FRAME_HELI_DUAL:
        case MOTOR_FRAME_HELI_QUAD:
            return MAV_TYPE_HELICOPTER;
        case MOTOR_FRAME_TRI:
            return MAV_TYPE_TRICOPTER;
        case MOTOR_FRAME_SINGLE:
        case MOTOR_FRAME_COAX:
        case MOTOR_FRAME_TAILSITTER:
            return MAV_TYPE_COAXIAL;
        case MOTOR_FRAME_DODECAHEXA:
            return MAV_TYPE_DODECAROTOR;
        case MOTOR_FRAME_DECA:
            return MAV_TYPE_DECAROTOR;
    }
    // unknown frame so return generic
    return MAV_TYPE_GENERIC;
}

MAV_MODE mavproxy_base_mode()
{
    Motors_HandleTypeDef *motors = fms.motors;

    uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
    // work out the base_mode. This value is not very useful
    // for APM, but we calculate it as best we can so a generic
    // MAVLink enabled ground station can work out something about
    // what the MAV is up to. The actual bit values are highly
    // ambiguous for most of the APM flight modes. In practice, you
    // only get useful information from the custom_mode, which maps to
    // the APM flight mode and has a well defined meaning in the
    // ArduPlane documentation
    switch (fms.flightmode->mode_number()) {
    case AUTO:
    case AUTO_RTL:
    case RTL:
    case LOITER:
    case AVOID_ADSB:
    case FOLLOW:
    case GUIDED:
    case CIRCLE:
    case POSHOLD:
    case BRAKE:
    case SMART_RTL:
        _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
        // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
        // APM does in any mode, as that is defined as "system finds its own goal
        // positions", which APM does not currently do
        break;
    default:
        break;
    }

    // all modes except INITIALISING have some form of manual
    // override if stick mixing is enabled
    _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;

    // we are armed if we are not initialising
    if (motors != NULL && motors->_armed) {
        _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
    }

    // indicate we have set a custom mode
    _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;

    return (MAV_MODE)_base_mode;
}

MAV_STATE mavproxy_vehicle_system_status()
{
    // set system as critical if any failsafe have triggered
    if (fms.failsafe.radio || 
        fms.failsafe.gcs || 
        fms.failsafe.ekf || 
        fms.failsafe.terrain || 
        fms.failsafe.adsb)  {
        return MAV_STATE_CRITICAL;
    }

    if (fms.ap.land_complete) {
        return MAV_STATE_STANDBY;
    }

    return MAV_STATE_ACTIVE;
}

uint32_t mavproxy_custom_mode()
{
    return (uint32_t)fms.flightmode->mode_number();
}

void mavproxy_send_banner()
{
    // mark the firmware version in the tlog
    mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s", "grapilot V0.0.1-dev");

    mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s: %s",
                  "OS", "RT-Thread");

    // send system ID if we can
    //char sysid[40];
    //mavproxy_send_statustext(MAV_SEVERITY_INFO, "%s", "");
}
/*------------------------------------test------------------------------------*/


